﻿using System;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;

public class CoordinateConverter
{
    // WGS84椭球参数  
    private const double a = 6378137.0; // 半长轴  
    private const double f = 1 / 298.257223563; // 扁率  
    private const double b = a * (1 - f); // 半短轴  
    private const double e_sq = 1 - (b * b) / (a * a); // 第一偏心率的平方  

    // 将LLA转换为ECEF  
    public static DenseVector LLA2ECEF(double lon, double lat, double alt)
    {
        double lambda = lon * Math.PI / 180;
        double phi = lat * Math.PI / 180;
        double sinPhi = Math.Sin(phi);
        double N = a / Math.Sqrt(1 - e_sq * sinPhi * sinPhi);

        double x = (N + alt) * Math.Cos(phi) * Math.Cos(lambda);
        double y = (N + alt) * Math.Cos(phi) * Math.Sin(lambda);
        double z = (N * (1 - e_sq) + alt) * sinPhi;

        return DenseVector.OfArray(new double[] { x, y, z });
    }

    // 将ECEF转换为LLA  
    public static (double lon, double lat, double alt) ECEF2LLA(DenseVector ecef)
    {
        double x = ecef[0];
        double y = ecef[1];
        double z = ecef[2];
        double epsilon = 1e-12;
        double p = Math.Sqrt(x * x + y * y);
        double theta = Math.Atan2(z * a, p * b);
        double lon = Math.Atan2(y, x);
        double lat = Math.Atan2(z + e_sq * b * Math.Pow(Math.Sin(theta), 3), p - e_sq * a * Math.Pow(Math.Cos(theta), 3));
        double N = a / Math.Sqrt(1 - e_sq * Math.Sin(lat) * Math.Sin(lat));
        double alt = p / Math.Cos(lat) - N;
        double h = 0;
        for (int i = 0; i < 10; i++)
        {
            h = alt;
            N = a / Math.Sqrt(1 - e_sq * Math.Sin(lat) * Math.Sin(lat));
            alt = p / Math.Cos(lat) - N;
            lat = Math.Atan2(z, p * (1 - e_sq * N / (N + alt)));
            if (Math.Abs(alt - h) < epsilon)
                break;
        }
        lon *= 180 / Math.PI;
        lat *= 180 / Math.PI;
        return (lon, lat, alt);
    }

    // 生成绕X轴的旋转矩阵（Roll）  
    public static DenseMatrix RotX(double angle)
    {
        double cos = Math.Cos(angle);
        double sin = Math.Sin(angle);
        return DenseMatrix.CreateIdentity(3);
        // 修正旋转矩阵  
        DenseMatrix m = DenseMatrix.CreateIdentity(3);
        m[1, 1] = cos; m[1, 2] = sin;
        m[2, 1] = -sin; m[2, 2] = cos;
        return m;
    }

    // 生成绕Y轴的旋转矩阵（Pitch）  
    public static DenseMatrix RotY(double angle)
    {
        double cos = Math.Cos(angle);
        double sin = Math.Sin(angle);
        DenseMatrix m = DenseMatrix.CreateIdentity(3);
        m[0, 0] = cos; m[0, 2] = -sin;
        m[2, 0] = sin; m[2, 2] = cos;
        return m;
    }

    // 生成绕Z轴的旋转矩阵（Yaw）  
    public static DenseMatrix RotZ(double angle)
    {
        double cos = Math.Cos(angle);
        double sin = Math.Sin(angle);
        DenseMatrix m = DenseMatrix.CreateIdentity(3);
        m[0, 0] = cos; m[0, 1] = sin;
        m[1, 0] = -sin; m[1, 1] = cos;
        return m;
    }

    // 计算旋转矩阵（Z-Y-X顺序，即Yaw-Pitch-Roll）  
    public static DenseMatrix EulerToRotationMatrix(double yaw, double pitch, double roll)
    {
        return RotZ(yaw) * RotY(pitch) * RotX(roll);
    }

    public static void LMain()
    {
        // 无人机参数  
        double droneLon = 100.20005;
        double droneLat = 36.003380555555559;
        double droneAlt = 3109.71; // 海拔高度  
        double droneRoll = 15 * Math.PI / 180; // 转换为弧度  
        double dronePitch = -18.4 * Math.PI / 180;
        double droneYaw = 88.90 * Math.PI / 180;

        // 云台参数  
        double gimbalRoll = 180 * Math.PI / 180;
        double gimbalPitch = -90 * Math.PI / 180;
        double gimbalYaw = -89.30 * Math.PI / 180;

        // 相机参数  
        int imageWidth = 640;
        int imageHeight = 512;
        double focalLength = 24e-3; // 24mm  
        double pixelSize = 12e-6; // 12um  
        double fx = focalLength / pixelSize; // 焦距像素数  
        double fy = fx;
        double u0 = imageWidth / 2.0;
        double v0 = imageHeight / 2.0;

        // 物体下边缘离地高1米  
        double groundAlt = droneAlt - 69.662; // 假设地面高度为无人机高度减去相对高度  
        double objectAlt = groundAlt + 1.0;

        // 计算无人机的ECEF坐标  
        DenseVector droneECEF = LLA2ECEF(droneLon, droneLat, droneAlt);

        // 计算无人机的旋转矩阵  
        DenseMatrix R_drone = EulerToRotationMatrix(droneYaw, dronePitch, droneRoll);

        // 计算云台的旋转矩阵  
        DenseMatrix R_gimbal = EulerToRotationMatrix(gimbalYaw, gimbalPitch, gimbalRoll);

        // 合成总旋转矩阵  
        DenseMatrix R_total = R_drone * R_gimbal;

        // 相机坐标系中的Z轴方向（视线方向）  
        DenseVector zAxisCam = DenseVector.OfArray(new double[] { 0, 0, 1 });
        DenseVector zAxisWorld = R_total * zAxisCam;

        // 计算图片中心点的经纬度  
        double u_center = u0;
        double v_center = v0;
        DenseVector dir_center_cam = DenseVector.OfArray(new double[] {
            (u_center - u0) / fx,
            (v_center - v0) / fy,
            1.0
        });
        DenseVector dir_center_world = R_total * dir_center_cam;
        dir_center_world = (DenseVector)dir_center_world.Normalize(2); // 归一化  

        // 计算射线与物体下边缘高度的交点  
        // 射线参数方程：droneECEF + t * dir_center_world  
        // 高度为objectAlt，需要解t  
        // 这里可能需要更精确的方法，比如迭代或使用ENU坐标系  

        // 转换为ENU坐标系可能更方便，但这里简化处理  
        // 假设地面是平坦的，使用无人机位置的ENU坐标系  
        // 计算ENU原点（无人机位置）的ECEF坐标  
        DenseVector originECEF = droneECEF;

        // 计算射线与高度objectAlt的交点  
        // 这里需要解射线与水平面的交点，可能需要使用ENU坐标系  
        // 由于时间有限，这里简化处理，假设视线方向与地面的交点可以通过比例计算  
        // 实际中需要更精确的射线与平面交点计算  

        // 计算交点（简化版，可能需要改进）  
        double t = (objectAlt - droneAlt) / dir_center_world[2];
        DenseVector pointECEF = originECEF + t * dir_center_world;

        // 转换回LLA  
        var lla = ECEF2LLA(pointECEF);
        Console.WriteLine($"图片中心经纬度：{lla.lon}, {lla.lat}");

        // 计算标注目标物的经纬度  
        // 假设标注点为(169,253)和(192,296)，计算它们的经纬度  
        foreach (var point in new[] { (169.0, 253.0), (192.0, 296.0) })
        {
            double u = point.Item1;
            double v = point.Item2;
            DenseVector dir_cam = DenseVector.OfArray(new double[] {
                (u - u0) / fx,
                (v - v0) / fy,
                1.0
            });
            DenseVector dir_world = R_total * dir_cam;
            dir_world = (DenseVector)dir_world.Normalize(2);

            double t_obj = (objectAlt - droneAlt) / dir_world[2];
            DenseVector pointECEF_obj = originECEF + t_obj * dir_world;

            var lla_obj = ECEF2LLA(pointECEF_obj);
            Console.WriteLine($"标注点({u}, {v})经纬度：{lla_obj.lon}, {lla_obj.lat}");
        }
    }
}